/**
 * @file        CANopen.c
 * @ingroup     CO_CANopen
 * @author      MAMAMA
 * @copyright   2010 - 2020 www.wingbow.com.cn
 */

#include "CANopen.h"

CanNodeInfo_T CanNodes[MAX_CAN_NODE] = {0};

static void _parse_heartbeat(CanRxMsg* msg);
static void _parse_sdo_tx(CanRxMsg* msg);
static void _parse_pdo1_tx(CanRxMsg* msg);

void canopen_init()
{
    int i = 0;
    for(i=0;i<MAX_CAN_NODE;i++)
        CanNodes[i].state = 0xFF;
}

bool canopen_parse_msg(CanRxMsg* msg)
{
    bool ret = true;
    uint16_t can_id = msg->StdId;
    if(can_id>=0x701 && can_id<=0x7FF)
        _parse_heartbeat(msg);
    else if(can_id>=0x581 && can_id<=0x5FF)
        _parse_sdo_tx(msg);
    else if(can_id>=0x181 && can_id<=0x1FF)
        _parse_pdo1_tx(msg);
    else
        ret = false;
    return ret;
}

static void _parse_heartbeat(CanRxMsg* msg)
{
    uint8_t node_id = msg->StdId - 0x700;
    if(node_id<=MAX_CAN_NODE)
    {
        CanNodes[node_id-1].id = node_id;
        CanNodes[node_id-1].state = msg->Data[0];
        CanNodes[node_id-1].id_type = CO_HEARTBEAT;
    }
}

static void _parse_sdo_tx(CanRxMsg* msg)
{
    uint8_t node_id = msg->StdId - 0x580;
    if(node_id<=MAX_CAN_NODE)
    {
        SDOTx_T* ack = (SDOTx_T*)msg->Data;
        if(ack->st_.CS==0x60)
        {
            CanNodes[node_id-1].manager.ptr++;
            if(CanNodes[node_id-1].manager.ptr==SDOConfigLen)
            {
                /* 理论上不会出现，SDOConfigs的最后一个数据必须是全零 */
                CanNodes[node_id-1].manager.state = SDO_ERR;
            }
            CanNodes[node_id-1].manager.state = SDO_GET_ACK;
        }
    }
}

static void _parse_pdo1_tx(CanRxMsg* msg)
{
    uint8_t node_id = msg->StdId - 0x180;
    if(node_id<=MAX_CAN_NODE)
    {
        /* User Define Code, 按照CANopen_config.c中映射的内容解析 */
        KincoStatus = *(KincoStatus_T*)msg->Data;
    }
}

/**
* @brief 发送一个SDO命令
*/
uint8_t canopen_sdo_tx(uint16_t node_id, SDOTx_T* sdo)
{
    if(sdo->st_.index==0x1800&&sdo->st_.sub_index==0x01)
        sdo->st_.data += node_id; //在这里加上TPDO COB-ID
    return CO_Send_LL(0x600+node_id, sdo->buf, 8);
}

/**
* @brief 发送一个NMT消息
* @param node_id 00表示广播
*        CS:  NMT_START-start remote node
*             NMT_STOP-stop remote node
*             others see CANopen_types.h
*/
uint8_t canopen_nmt(uint8_t node_id, uint8_t CS)
{
    uint8_t nmt[2] = {0};
    nmt[0] = node_id;
    nmt[1] = CS;
    return CO_Send_LL(0x000, nmt, 2);
}

uint8_t canopen_sync()
{
    return CO_Send_LL(0x80, (void*)0, 0);
}

void sdo_loop()
{
    int i;
    for(i=0;i<MAX_CAN_NODE;i++)
    {
        if(CanNodes[i].id!=0)
        {
            uint8_t index = CanNodes[i].manager.ptr;
            if(SDOConfigs[index].st_.index!=0)
            {
                if(CanNodes[i].manager.state==SDO_IDLE ||
                    CanNodes[i].manager.state==SDO_GET_ACK)
                {
                    CanNodes[i].manager.state = SDO_SENDING;
                    if(canopen_sdo_tx(CanNodes[i].id, &SDOConfigs[index])==CAN_OK)
                        CanNodes[i].manager.state = SDO_WAIT_ACK;
                }
            }
            else
            {
                if(CanNodes[i].manager.state==SDO_GET_ACK)
                {
                    /* SDO配置完成，可以设置NMT启动节点了 */
                    CanNodes[i].manager.state = SDO_WAIT_NMT;
                }
            }
        }
    }
}

void nmt_loop()
{
    int i;
    for(i=0;i<MAX_CAN_NODE;i++)
    {
        if(CanNodes[i].id!=0)
        {
            if(CanNodes[i].manager.state==SDO_WAIT_NMT)
            {
                canopen_nmt(CanNodes[i].id, NMT_START);
                CanNodes[i].manager.state = SDO_FINISH;
            }
        }
    }
}

